#pragma once
struct Matrix3x4;

//
// Matrix
//
class Matrix
{
public:
	union
	{
		MS_ALIGN(16) float M[4][4];
	};
	
	static const Matrix Identity;
	
	Matrix() {}
	Matrix(const float* ptr);
	Matrix(const Vector3& x,const Vector3& y,const Vector3& z,const Vector3& w);
	Matrix(const Vector4& x,const Vector4& y,const Vector4& z,const Vector4& w);
	
	float* Ptr() { return M[0]; }
	const float* Ptr() const { return M[0]; }

	float* operator[](int i) { return M[i]; }
	const float* operator[](int i) const { return M[i]; }

	Matrix operator* (const Matrix& Other) const;
	void operator*=(const Matrix& Other);

	Vector3 GetAxis(int i) const { return Vector3(M[i][0], M[i][1], M[i][2]); }
	void SetAxis(int i, const Vector3& v) { M[i][0] = v.X; M[i][1] = v.Y; M[i][2] = v.Z; }
	Rotator getRotator() const;

	void SetIdentity();
	void CopyFrom(const void* data);
	Matrix Inverse() const;
	Matrix InverseSafe() const;

	Vector4 Transform(const Vector4& v) const;
	Vector4 Transform(const Vector3& v) const;
	Vector3 TransformAffine(const Vector3& v) const;
	Vector4 TransformNormal(const Vector3& v) const;

	// Frustum plane extraction.
	bool GetFrustumNearPlane(Plane& OutPlane) const;
	bool GetFrustumFarPlane(Plane& OutPlane) const;
	bool GetFrustumLeftPlane(Plane& OutPlane) const;
	bool GetFrustumRightPlane(Plane& OutPlane) const;
	bool GetFrustumTopPlane(Plane& OutPlane) const;
	bool GetFrustumBottomPlane(Plane& OutPlane) const;

	// helper functions.
	static Matrix TranslationMatrix(const Vector3& T);
	static Matrix RotationMatrix(const Rotator& R);
	static Matrix InverseRotationMatrix(const Rotator& R);
	static Matrix RotationTranslationMatrix(const Quaternion& Q, const Vector3& T);
	static Matrix RotationTranslationMatrix(const Rotator& R, const Vector3& T);
	static Matrix ScaleRotationTranslationMatrix(const Vector3& S, const Quaternion& Q, const Vector3& T);
	static Matrix ScaleRotationTranslationMatrix(const Vector3& S, const Rotator& R, const Vector3& T);
	static Matrix PerspectiveMatrix(float FOV, float Aspect, float MinZ, float MaxZ);
};

class Matrix3
{
public:
	Vector3 M[3];

	Matrix3() {}
	Matrix3(const Quaternion& q)
	{
		q.ToRotationMatrix(*this);
	}

	void SetIdentity(){
		M[0] = Vector3::UnitX;
		M[1] = Vector3::UnitY;
		M[2] = Vector3::UnitZ;
	}
	
	Vector3& operator[](int i) { return M[i]; }
	const Vector3& operator[](int i) const { return M[i]; }

	Vector3 operator*(const Vector3& v) const
	{
        return Vector3(M[0] | v, M[1] | v, M[2] | v);
	}

	Matrix3 operator*(const Matrix3& v) const
	{
        Matrix3 kProd;
        for (size_t iRow = 0; iRow < 3; iRow++)
        {
            for (size_t iCol = 0; iCol < 3; iCol++)
            {
                kProd.M[iRow][iCol] =
                    M[iRow][0] * v.M[0][iCol] +
                    M[iRow][1] * v.M[1][iCol] +
                    M[iRow][2] * v.M[2][iCol];
            }
        }
        return kProd;
	}

	Matrix3 Transpose() const
	{
        Matrix3 kProd = *this;
		Swap(kProd.M[0][1], kProd.M[1][0]);
		Swap(kProd.M[0][2], kProd.M[2][0]);
		Swap(kProd.M[1][2], kProd.M[2][1]);
		return kProd;
	}
};

//
// Matrix3x4
//	for GPU skinning.
struct Matrix3x4
{
	float M[3][4];
	void operator=(const Matrix& m)
	{
		M[0][0] = m[0][0]; M[1][0] = m[0][1]; M[2][0] = m[0][2];
		M[0][1] = m[1][0]; M[1][1] = m[1][1]; M[2][1] = m[1][2];
		M[0][2] = m[2][0]; M[1][2] = m[2][1]; M[2][2] = m[2][2];
		M[0][3] = m[3][0]; M[1][3] = m[3][1]; M[2][3] = m[3][2];
	}
};

inline Matrix::Matrix(const float* ptr)
{
	CopyFrom(ptr);
}

inline Matrix::Matrix(const Vector3& x,const Vector3& y,const Vector3& z,const Vector3& w)
{
	M[0][0] = x.X; M[0][1] = x.Y;  M[0][2] = x.Z;  M[0][3] = 0;
	M[1][0] = y.X; M[1][1] = y.Y;  M[1][2] = y.Z;  M[1][3] = 0;
	M[2][0] = z.X; M[2][1] = z.Y;  M[2][2] = z.Z;  M[2][3] = 0;
	M[3][0] = w.X; M[3][1] = w.Y;  M[3][2] = w.Z;  M[3][3] = 1;
}

inline Matrix::Matrix(const Vector4& x,const Vector4& y,const Vector4& z,const Vector4& w)
{
	M[0][0] = x.X; M[0][1] = x.Y;  M[0][2] = x.Z;  M[0][3] = x.W;
	M[1][0] = y.X; M[1][1] = y.Y;  M[1][2] = y.Z;  M[1][3] = y.W;
	M[2][0] = z.X; M[2][1] = z.Y;  M[2][2] = z.Z;  M[2][3] = z.W;
	M[3][0] = w.X; M[3][1] = w.Y;  M[3][2] = w.Z;  M[3][3] = w.W;
}

inline Matrix Matrix::operator* (const Matrix& Other) const
{
	Matrix Result;
	VectorMatrixMultiply( &Result, this, &Other );
	return Result;
}

inline void Matrix::operator*=(const Matrix& Other)
{
	VectorMatrixMultiply(Ptr(), Ptr(), Other.Ptr());
}

inline void Matrix::SetIdentity()
{
	M[0][0] = 1; M[0][1] = 0;  M[0][2] = 0;  M[0][3] = 0;
	M[1][0] = 0; M[1][1] = 1;  M[1][2] = 0;  M[1][3] = 0;
	M[2][0] = 0; M[2][1] = 0;  M[2][2] = 1;  M[2][3] = 0;
	M[3][0] = 0; M[3][1] = 0;  M[3][2] = 0;  M[3][3] = 1;
}

inline void Matrix::CopyFrom(const void* data)
{
	memcpy(Ptr(), data, sizeof(Matrix));
}

inline Matrix Matrix::InverseSafe() const
{
	if (GetAxis(0).IsNearlyZero() && GetAxis(1).IsNearlyZero() && GetAxis(2).IsNearlyZero())
		return Matrix(Vector3(0.0f), Vector3(0.0f), Vector3(0.0f), Vector3(0.0f));
	return Inverse();
}

inline Vector4 Matrix::Transform(const Vector4& P) const
{
	Vector4 Result;
	Result.X = P.X * M[0][0] + P.Y * M[1][0] + P.Z * M[2][0] + P.W * M[3][0];
	Result.Y = P.X * M[0][1] + P.Y * M[1][1] + P.Z * M[2][1] + P.W * M[3][1];
	Result.Z = P.X * M[0][2] + P.Y * M[1][2] + P.Z * M[2][2] + P.W * M[3][2];
	Result.W = P.X * M[0][3] + P.Y * M[1][3] + P.Z * M[2][3] + P.W * M[3][3];
	return Result;
}

inline Vector4 Matrix::Transform(const Vector3& v) const
{
    return Transform(Vector4(v, 1));
}

inline Vector3 Matrix::TransformAffine(const Vector3& v) const
{
	Vector4 Result = Transform(Vector4(v, 1));
	return Vector3(Result.X, Result.Y, Result.Z);
}

inline Vector4 Matrix::TransformNormal(const Vector3& v) const
{
    return Transform(Vector4(v, 0));
}

	
inline Matrix Matrix::TranslationMatrix(const Vector3& T)
{
	Matrix M;
	M[0][0] = 1;	M[0][1] = 0;	M[0][2] = 0;	M[0][3] = 0;
	M[1][0] = 0;	M[1][1] = 1;	M[1][2] = 0;	M[1][3] = 0;
	M[2][0] = 0;	M[2][1] = 0;	M[2][2] = 1;	M[2][3] = 0;
	M[3][0] = T.X;	M[3][1] = T.Y;	M[3][2] = T.Z;	M[3][3] = 1;
	return M;
}

inline Matrix Matrix::RotationMatrix(const Rotator& R)
{
	const FLOAT	SR	= GMath.SinTab(R.Z);
	const FLOAT	SP	= GMath.SinTab(R.X);
	const FLOAT	SY	= GMath.SinTab(R.Y);
	const FLOAT	CR	= GMath.CosTab(R.Z);
	const FLOAT	CP	= GMath.CosTab(R.X);
	const FLOAT	CY	= GMath.CosTab(R.Y);

	Matrix M;
	M[0][0]	= CP * CY;
	M[0][1]	= CP * SY;
	M[0][2]	= SP;
	M[0][3]	= 0.f;

	M[1][0]	= SR * SP * CY - CR * SY;
	M[1][1]	= SR * SP * SY + CR * CY;
	M[1][2]	= - SR * CP;
	M[1][3]	= 0.f;

	M[2][0]	= -( CR * SP * CY + SR * SY );
	M[2][1]	= CY * SR - CR * SP * SY;
	M[2][2]	= CR * CP;
	M[2][3]	= 0.f;

	M[3][0]	= 0.f;
	M[3][1]	= 0.f;
	M[3][2]	= 0.f;
	M[3][3]	= 1.f;
	return M;
}

inline Matrix Matrix::InverseRotationMatrix(const Rotator& R)
{
	return Matrix(	// Yaw
			Vector4(+GMath.CosTab(-R.Y),	+GMath.SinTab(-R.Y),	0.0f,					0.0f),
			Vector4(-GMath.SinTab(-R.Y),	+GMath.CosTab(-R.Y),	0.0f,					0.0f),
			Vector4(0.0f,					0.0f,					1.0f,					0.0f),
			Vector4(0.0f,					0.0f,					0.0f,					1.0f)) *
		Matrix(	// Pitch
			Vector4(+GMath.CosTab(-R.X),	0.0f,					+GMath.SinTab(-R.X),	0.0f),
			Vector4(0.0f,					1.0f,					0.0f,					0.0f),
			Vector4(-GMath.SinTab(-R.X),	0.0f,					+GMath.CosTab(-R.X),	0.0f),
			Vector4(0.0f,					0.0f,					0.0f,					1.0f)) *
		Matrix(	// Roll
			Vector4(1.0f,					0.0f,					0.0f,					0.0f),
			Vector4(0.0f,					+GMath.CosTab(-R.Z),	-GMath.SinTab(-R.Z),	0.0f),
			Vector4(0.0f,					+GMath.SinTab(-R.Z),	+GMath.CosTab(-R.Z),	0.0f),
			Vector4(0.0f,					0.0f,					0.0f,					1.0f));
}

inline Matrix Matrix::RotationTranslationMatrix(const Quaternion& Q, const Vector3& T)
{
	const FLOAT x2 = Q.X + Q.X;  const FLOAT y2 = Q.Y + Q.Y;  const FLOAT z2 = Q.Z + Q.Z;
	const FLOAT xx = Q.X * x2;   const FLOAT xy = Q.X * y2;   const FLOAT xz = Q.X * z2;
	const FLOAT yy = Q.Y * y2;   const FLOAT yz = Q.Y * z2;   const FLOAT zz = Q.Z * z2;
	const FLOAT wx = Q.W * x2;   const FLOAT wy = Q.W * y2;   const FLOAT wz = Q.W * z2;

	Matrix M;
	M[0][0] = 1.0f - (yy + zz);	M[1][0] = xy - wz;				M[2][0] = xz + wy;			M[3][0] = T.X;
	M[0][1] = xy + wz;			M[1][1] = 1.0f - (xx + zz);		M[2][1] = yz - wx;			M[3][1] = T.Y;
	M[0][2] = xz - wy;			M[1][2] = yz + wx;				M[2][2] = 1.0f - (xx + yy);	M[3][2] = T.Z;
	M[0][3] = 0.0f;				M[1][3] = 0.0f;					M[2][3] = 0.0f;				M[3][3] = 1.0f;
	return M;
}

inline Matrix Matrix::ScaleRotationTranslationMatrix(const Vector3& S, const Quaternion& Q, const Vector3& T)
{
#if 1
	Matrix OutMatrix;
	OutMatrix.M[3][0] = T.X;
	OutMatrix.M[3][1] = T.Y;
	OutMatrix.M[3][2] = T.Z;

	const FLOAT x2 = Q.X + Q.X;	
	const FLOAT y2 = Q.Y + Q.Y;  
	const FLOAT z2 = Q.Z + Q.Z;
	{
		const FLOAT xx2 = Q.X * x2;
		const FLOAT yy2 = Q.Y * y2;			
		const FLOAT zz2 = Q.Z * z2;

		OutMatrix.M[0][0] = (1.0f - (yy2 + zz2)) * S.X;	
		OutMatrix.M[1][1] = (1.0f - (xx2 + zz2)) * S.Y;
		OutMatrix.M[2][2] = (1.0f - (xx2 + yy2)) * S.Z;
	}
	{
		const FLOAT yz2 = Q.Y * z2;   
		const FLOAT wx2 = Q.W * x2;	

		OutMatrix.M[2][1] = (yz2 - wx2) * S.Z;
		OutMatrix.M[1][2] = (yz2 + wx2) * S.Y;
	}
	{
		const FLOAT xy2 = Q.X * y2;
		const FLOAT wz2 = Q.W * z2;

		OutMatrix.M[1][0] = (xy2 - wz2) * S.Y;
		OutMatrix.M[0][1] = (xy2 + wz2) * S.X;
	}
	{
		const FLOAT xz2 = Q.X * z2;
		const FLOAT wy2 = Q.W * y2;   

		OutMatrix.M[2][0] = (xz2 + wy2) * S.Z;
		OutMatrix.M[0][2] = (xz2 - wy2) * S.X;
	}

	OutMatrix.M[0][3] = 0.0f;
	OutMatrix.M[1][3] = 0.0f;
	OutMatrix.M[2][3] = 0.0f;
	OutMatrix.M[3][3] = 1.0f;
	return OutMatrix;
#else
	const FLOAT x2 = Q.X + Q.X;  const FLOAT y2 = Q.Y + Q.Y;  const FLOAT z2 = Q.Z + Q.Z;
	const FLOAT xx = Q.X * x2;   const FLOAT xy = Q.X * y2;   const FLOAT xz = Q.X * z2;
	const FLOAT yy = Q.Y * y2;   const FLOAT yz = Q.Y * z2;   const FLOAT zz = Q.Z * z2;
	const FLOAT wx = Q.W * x2;   const FLOAT wy = Q.W * y2;   const FLOAT wz = Q.W * z2;

	Matrix M;
	M[0][0] = (1.0f - (yy + zz)) * S.X;
	M[1][0] = (xy - wz) * S.Y;
	M[2][0] = (xz + wy) * S.Z;
	M[3][0] = T.X;

	M[0][1] = (xy + wz) * S.X;
	M[1][1] = (1.0f - (xx + zz)) * S.Y;
	M[2][1] = (yz - wx) * S.Z;
	M[3][1] = T.Y;

	M[0][2] = (xz - wy) * S.X;
	M[1][2] = (yz + wx) * S.Y;
	M[2][2] = (1.0f - (xx + yy)) * S.Z;
	M[3][2] = T.Z;

	M[0][3] = 0.0f;
	M[1][3] = 0.0f;
	M[2][3] = 0.0f;
	M[3][3] = 1.0f;
	return M;
#endif
}

inline Matrix Matrix::RotationTranslationMatrix(const Rotator& R, const Vector3& T)
{
	const FLOAT	SR	= GMath.SinTab(R.Z);
	const FLOAT	SP	= GMath.SinTab(R.X);
	const FLOAT	SY	= GMath.SinTab(R.Y);
	const FLOAT	CR	= GMath.CosTab(R.Z);
	const FLOAT	CP	= GMath.CosTab(R.X);
	const FLOAT	CY	= GMath.CosTab(R.Y);

	Matrix M;
	M[0][0]	= CP * CY;
	M[0][1]	= CP * SY;
	M[0][2]	= SP;
	M[0][3]	= 0.f;

	M[1][0]	= SR * SP * CY - CR * SY;
	M[1][1]	= SR * SP * SY + CR * CY;
	M[1][2]	= - SR * CP;
	M[1][3]	= 0.f;

	M[2][0]	= -( CR * SP * CY + SR * SY );
	M[2][1]	= CY * SR - CR * SP * SY;
	M[2][2]	= CR * CP;
	M[2][3]	= 0.f;

	M[3][0]	= T.X;
	M[3][1]	= T.Y;
	M[3][2]	= T.Z;
	M[3][3]	= 1.f;
	return M;
}

inline Matrix Matrix::ScaleRotationTranslationMatrix(const Vector3& S, const Rotator& R, const Vector3& T)
{
	const FLOAT	SR	= GMath.SinTab(R.Z);
	const FLOAT	SP	= GMath.SinTab(R.X);
	const FLOAT	SY	= GMath.SinTab(R.Y);
	const FLOAT	CR	= GMath.CosTab(R.Z);
	const FLOAT	CP	= GMath.CosTab(R.X);
	const FLOAT	CY	= GMath.CosTab(R.Y);

	Matrix M;
	M[0][0]	= (CP * CY) * S.X;
	M[0][1]	= (CP * SY) * S.X;
	M[0][2]	= (SP) * S.X;
	M[0][3]	= 0.f;

	M[1][0]	= (SR * SP * CY - CR * SY) * S.Y;
	M[1][1]	= (SR * SP * SY + CR * CY) * S.Y;
	M[1][2]	= (- SR * CP) * S.Y;
	M[1][3]	= 0.f;

	M[2][0]	= ( -( CR * SP * CY + SR * SY ) ) * S.Z;
	M[2][1]	= (CY * SR - CR * SP * SY) * S.Z;
	M[2][2]	= (CR * CP) * S.Z;
	M[2][3]	= 0.f;

	M[3][0]	= T.X;
	M[3][1]	= T.Y;
	M[3][2]	= T.Z;
	M[3][3]	= 1.f;
	return M;
}

inline Matrix Matrix::PerspectiveMatrix(float FOV, float Aspect, float MinZ, float MaxZ)
{
	Matrix M;
	M[0][0] = 1 / tanf(FOV);	M[0][1] = 0;					M[0][2] = 0;				M[0][3] = 0;
	M[1][0] = 0;				M[1][1] = Aspect / tanf(FOV);	M[1][2] = 0;				M[1][3] = 0;
	M[2][0] = 0;				M[2][1] = 0;				M[2][2] = MaxZ / (MaxZ - MinZ);	M[2][3] = 1;
	M[3][0] = 0;				M[3][1] = 0;	M[3][2] = -MinZ * (MaxZ / (MaxZ - MinZ));	M[3][3] = 0;
	return M;
}

inline Rotator Matrix::getRotator() const
{
	const Vector3 XAxis	= GetAxis( 0 );
	const Vector3 YAxis	= GetAxis( 1 );
	const Vector3 ZAxis	= GetAxis( 2 );

	Rotator rotator(
		Math::Round(atan2f( XAxis.Z, sqrt(Square(XAxis.X)+Square(XAxis.Y)) ) * 32768.f / PI), 
		Math::Round(atan2f( XAxis.Y, XAxis.X ) * 32768.f / PI), 
		0);
	
	const Vector3 SYAxis = Matrix::RotationMatrix( rotator ).GetAxis(1);
	rotator.Z = Math::Round(atan2f( ZAxis | SYAxis, YAxis | SYAxis ) * 32768.f / PI);
	return rotator;
}

inline bool MakeFrustumPlane(float A, float B, float C, float D, Plane& OutPlane)
{
	const float DELTA = 0.00001f;
	const float	LengthSquared = A * A + B * B + C * C;
	if (LengthSquared > DELTA*DELTA)
	{
		const FLOAT	InvLength = GMath.InvSqrt(LengthSquared);
		OutPlane = Plane(-A * InvLength,-B * InvLength,-C * InvLength,D * InvLength);
		return 1;
	}
	else
		return 0;
}

// Frustum plane extraction.
inline bool Matrix::GetFrustumNearPlane(Plane& OutPlane) const
{
	return MakeFrustumPlane(
		M[0][2],
		M[1][2],
		M[2][2],
		M[3][2],
		OutPlane);
}

inline bool Matrix::GetFrustumFarPlane(Plane& OutPlane) const
{
	return MakeFrustumPlane(
		M[0][3] - M[0][2],
		M[1][3] - M[1][2],
		M[2][3] - M[2][2],
		M[3][3] - M[3][2],
		OutPlane);
}

inline bool Matrix::GetFrustumLeftPlane(Plane& OutPlane) const
{
	return MakeFrustumPlane(
		M[0][3] + M[0][0],
		M[1][3] + M[1][0],
		M[2][3] + M[2][0],
		M[3][3] + M[3][0],
		OutPlane);
}

inline bool Matrix::GetFrustumRightPlane(Plane& OutPlane) const
{
	return MakeFrustumPlane(
		M[0][3] - M[0][0],
		M[1][3] - M[1][0],
		M[2][3] - M[2][0],
		M[3][3] - M[3][0],
		OutPlane);
}

inline bool Matrix::GetFrustumTopPlane(Plane& OutPlane) const
{
	return MakeFrustumPlane(
		M[0][3] - M[0][1],
		M[1][3] - M[1][1],
		M[2][3] - M[2][1],
		M[3][3] - M[3][1],
		OutPlane);
}

inline bool Matrix::GetFrustumBottomPlane(Plane& OutPlane) const
{
	return MakeFrustumPlane(
		M[0][3] + M[0][1],
		M[1][3] + M[1][1],
		M[2][3] + M[2][1],
		M[3][3] + M[3][1],
		OutPlane);
}
